// IArduinoEasyStar
// by iEagles 2010 <http://www.iEagles.net> 
//

#include <SoftwareServo.h> 
//#include <Servo.h> 
 
#include "WProgram.h"
void setup();
void loop();
SoftwareServo myServoProfondeur;            // Profondeur
SoftwareServo myServoDerive;            // Derive
SoftwareServo mySpeedController;   // Controleur de vitesse

byte iNumberOfBytes = 0;
byte iReceivedByte1 = 0;
byte iReceivedByte2 = 0;

int  m_iBufferSize    = 6;  // The size of the received buffer
byte m_ReceivedBuffer[6];
byte m_iSeparationByte = 255;
byte m_iEchoMsg = 1;
boolean m_bIsBufferOK = false;
boolean bAllowSending = true;

/*Cummunication protocol
1er  :0 pour la s\u00e9paration \u2018255\u2019
2ieme:1 type de commande [0 send only, 1 send with echo]
3i\u00e8me:2 controleur du moteur
4i\u00e8me:3 servomoteur la direction
5i\u00e8me:4 servomoteur de la profondeur.
6i\u00e8me:5 check sum*/

void setup() 
{ 
  //Configure the pwm 
  myServoProfondeur.attach(3);  // attaches the servo on pin 9 to the servo object 
  myServoDerive.attach(5);
  mySpeedController.attach(6);
  
  myServoProfondeur.setMaximumPulse(2400);
  myServoDerive.setMaximumPulse(2400);
  //mySpeedController.setMaximumPulse(2200);
  
  myServoProfondeur.setMinimumPulse(600);
  myServoDerive.setMinimumPulse(600);
  //mySpeedController.setMinimumPulse(600);
  
  //Serial.begin(115200);  
  //Serial.begin(9600);  
  Serial.begin(38400);

  Serial.println("iEagles.net 2010");
} 
 
 
void loop() 
{  
  iNumberOfBytes =Serial.available();
  
  /*  //Echo test
    if(iNumberOfBytes>0){
    iReceivedByte1 = Serial.read();
    Serial.print(iReceivedByte1);
  }*/
 
  //Receive the buffer
  if ( iNumberOfBytes >= m_iBufferSize && bAllowSending == true) {
    //Serial.print("iNumberOfBytes = ");
    //Serial.println(iNumberOfBytes,DEC);
    
    //Check that there is a separation
    m_ReceivedBuffer[0] = 0;
    m_ReceivedBuffer[0] = Serial.read();
    m_bIsBufferOK = false; 
    
    if(m_ReceivedBuffer[0] == m_iSeparationByte){
    //if(true){
      //We got a separation byte, we read the next part of the message
      m_ReceivedBuffer[1] = Serial.read();//delay(1);//Type de commande
      m_ReceivedBuffer[2] = Serial.read();//delay(1);//Moteur
      m_ReceivedBuffer[3] = Serial.read();//delay(1);//D\u00e9rive
      m_ReceivedBuffer[4] = Serial.read();//delay(1);//Profondeur
      m_ReceivedBuffer[5] = Serial.read();//delay(1);//CheckSum   
      
      //Check that the received message is coherent
      m_bIsBufferOK = true;
    
      if(m_bIsBufferOK){
      
        if((int)m_ReceivedBuffer[3]>180 || (int)m_ReceivedBuffer[4] >180){
          //delay(25);
          }else{      
            mySpeedController.write(m_ReceivedBuffer[2]);
            myServoDerive.write(m_ReceivedBuffer[3]);
            myServoProfondeur.write(m_ReceivedBuffer[4]);
          }     

        //Sendback the message
        if(m_ReceivedBuffer[1] == m_iEchoMsg){
          //Sending back echo - To send upon request 
          Serial.write(m_ReceivedBuffer[0]);//delay(1);
          Serial.write(m_ReceivedBuffer[1]);//delay(1);
          Serial.write(m_ReceivedBuffer[2]);//delay(1);
          Serial.write(m_ReceivedBuffer[3]);//delay(1);
          Serial.write(m_ReceivedBuffer[4]);//delay(1);
          Serial.write(m_ReceivedBuffer[5]);//delay(1);
        }
      }  
  
    }else{
      return; 
    } 
    
  }else{

  }  
  
  SoftwareServo::refresh();
} 

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

